; Define tolerance for position errors (in mm)
;var tolerance = 0.5  ; COMMENTED OUT - Motion Accuracy on hold
var num_iterations = 50
;var checkEvery = 5  ; Check accuracy every N iterations - COMMENTED OUT - Motion Accuracy on hold

M291 S5 J1 F350 R"XY Movement test" P"Set the temperature for the Test"
var temp = input


M564 H0
G10 P0 S{var.temp} R{var.temp}
G10 P1 S{var.temp} R{var.temp}
G10 P2 S{var.temp} R{var.temp}
G10 P3 S{var.temp} R{var.temp}

M140 S150 R150
M141 S100 R100

; Homing XYU axis
if !move.axes[0].homed || !move.axes[1].homed || !move.axes[2].homed || !move.axes[3].homed
  M98 P"homeall.g"                    ; Home all if any axis is not homed
else
  G28 Y                               ; Home Y axis
  G28 X                               ; Home X axis
  G28 U                               ; Home U axis

G90
G1 Z300 F18000

;Wait for temp
M116 H0 S5
M116 H1 S5

M570 H0 P1 T10 R1                                            ; configure heater fault detection
M570 H1 P1 T10 R1                                            ; configure heater fault detection


; Fans
M106 P1 S0.5
M106 P3 S0.5

;M98 P"0:/macros/System/Calibration/QC/Tests/Motion Accuracy"
M400




; ---- CSV header setup ----
; CSV header for temperature logging during movement test
echo >"0:/sys/logs/Motion_Accuracy_Test.csv" "Iteration,Elapsed_Seconds,Tool0_Temp,Tool1_Temp,Bed_Temp,Chamber_Temp"

; Variables for CSV logging
var csvData = ""

; Temperature sensor variables
var t0 = 0
var t1 = 0
var bed = 0
var chamber = 0

; Start time tracking
var XY_Mov_Start_Time = state.time
var XY_Mov_Time = 0

; COMMENTED OUT - Motion Accuracy Test on hold
;pull data from global variable and use it as default values 
;if !exists(global.axisPositions)
;    abort "Error: Motion Accuracy must be run first to establish baseline measurements"

; Store baseline measurements as reference values
;var baselineX = global.axisPositions[0]
;var baselineLeftY = global.axisPositions[1]
;var baselineRightY = global.axisPositions[2]
;var baselineU = global.axisPositions[3]

; Dummy baseline values since Motion Accuracy is disabled
;var baselineX = 0
;var baselineLeftY = 0
;var baselineRightY = 0
;var baselineU = 0

; Error flag variables
;var errorDetected = false
;var errorAxis = ""
;var errorValue = 0

;=echo "Baseline measurements established:"
;=echo "X-axis: " ^ {var.baselineX} ^ " mm"
;=echo "Y-axis Left: " ^ {var.baselineLeftY} ^ " mm"
;=echo "Y-axis Right: " ^ {var.baselineRightY} ^ " mm"
;=echo "U-axis: " ^ {var.baselineU} ^ " mm"


; SIMPLIFIED LOOP - Temperature logging only, no motion accuracy checks
while sensors.analog[3].lastReading <= 80 && iterations < {var.num_iterations}
    
    M98 P"0:/macros/System/Calibration/QC/Tests/Motion Script"
    M400
    ;M98 P"0:/macros/System/Calibration/QC/Tests/Motion Accuracy"
    M400
    
    ; Read temperature sensors
    set var.t0 = sensors.analog[0].lastReading      ; Tool 0
    set var.t1 = sensors.analog[1].lastReading      ; Tool 1  
    set var.bed = sensors.analog[2].lastReading     ; Build Plate
    set var.chamber = sensors.analog[3].lastReading ; Chamber Air
    
    ; Calculate elapsed time
    set var.XY_Mov_Time = state.time - var.XY_Mov_Start_Time
    
    ; Report temperatures for this iteration
    echo "Iteration " ^ {iterations + 1} ^ " - Tool0: " ^ {var.t0} ^ "°C, Tool1: " ^ {var.t1} ^ "°C, Bed: " ^ {var.bed} ^ "°C, Chamber: " ^ {var.chamber} ^ "°C"
    
    ; Log data to CSV - elapsed time and all temperatures
    set var.csvData = {iterations + 1} ^ "," ^ {floor(var.XY_Mov_Time)} ^ "," ^ {var.t0} ^ "," ^ {var.t1} ^ "," ^ {var.bed} ^ "," ^ {var.chamber}
    echo >>"0:/sys/logs/Motion_Accuracy_Test.csv" {var.csvData}

; COMMENTED OUT - All motion accuracy checking logic (position measurements, deviations, error detection)
;    ;pull data from global variable and compare with default values, report Error with echo
;    if exists(global.axisPositions)
;        ; Read temperature sensors
;        set var.t0 = sensors.analog[0].lastReading      ; Tool 0
;        set var.t1 = sensors.analog[1].lastReading      ; Tool 1  
;        set var.bed = sensors.analog[2].lastReading     ; Build Plate
;        set var.chamber = sensors.analog[3].lastReading ; Chamber Air
;        
;        ; Calculate elapsed time
;        set var.XY_Mov_Time = state.time - var.XY_Mov_Start_Time
;        
;        ; Check accuracy only every N iterations
;        if mod(iterations + 1, var.checkEvery) == 0
;            var currentX = global.axisPositions[0]
;            var currentLeftY = global.axisPositions[1]
;            var currentRightY = global.axisPositions[2]
;            var currentU = global.axisPositions[3]
;            
;            ; Calculate deviations from baseline
;            var deviationX = abs(var.currentX - var.baselineX)
;            var deviationLeftY = abs(var.currentLeftY - var.baselineLeftY)
;            var deviationRightY = abs(var.currentRightY - var.baselineRightY)
;            var deviationU = abs(var.currentU - var.baselineU)
;            
;            ; Report for each axis - Iteration and error values
;            echo "Iteration " ^ {iterations + 1} ^ " (Accuracy Check)"
;            echo {var.deviationX * 1000} ^ "μm (" ^ {var.deviationX} ^ "mm) - X-axis"
;            echo {var.deviationLeftY * 1000} ^ "μm (" ^ {var.deviationLeftY} ^ "mm) - Left Y-axis"
;            echo {var.deviationRightY * 1000} ^ "μm (" ^ {var.deviationRightY} ^ "mm) - Right Y-axis"
;            echo {var.deviationU * 1000} ^ "μm (" ^ {var.deviationU} ^ "mm) - U-axis"
;            echo {var.t0} ^ "°C - Tool0"
;            echo {var.t1} ^ "°C - Tool1"
;            echo {var.bed} ^ "°C - Bed"
;            echo {var.chamber} ^ "°C - Chamber"
;            
;            ; Log data to CSV - include elapsed time, all errors, and all temperatures
;            set var.csvData = {iterations + 1} ^ "," ^ {floor(var.XY_Mov_Time)} ^ "," ^ {var.deviationX * 1000} ^ "," ^ {var.deviationLeftY * 1000} ^ "," ^ {var.deviationRightY * 1000} ^ "," ^ {var.deviationU * 1000}
;            set var.csvData = var.csvData ^ "," ^ {var.t0} ^ "," ^ {var.t1} ^ "," ^ {var.bed} ^ "," ^ {var.chamber}
;            echo >>"0:/sys/logs/Motion_Accuracy_Test.csv" {var.csvData}
;            
;            ; Check for errors and set flag if tolerance exceeded
;            if var.deviationX > var.tolerance
;                echo "ERROR: Deviation detected on X-axis: " ^ {var.deviationX} ^ "mm (tolerance: " ^ {var.tolerance} ^ "mm)"
;                set var.errorDetected = true
;                set var.errorAxis = "X-axis"
;                set var.errorValue = var.deviationX
;                break
;            
;            if var.deviationLeftY > var.tolerance
;                echo "ERROR: Deviation detected on Y-axis Left: " ^ {var.deviationLeftY} ^ "mm (tolerance: " ^ {var.tolerance} ^ "mm)"
;                set var.errorDetected = true
;                set var.errorAxis = "Y-axis Left"
;                set var.errorValue = var.deviationLeftY
;                break
;            
;            if var.deviationRightY > var.tolerance
;                echo "ERROR: Deviation detected on Y-axis Right: " ^ {var.deviationRightY} ^ "mm (tolerance: " ^ {var.tolerance} ^ "mm)"
;                set var.errorDetected = true
;                set var.errorAxis = "Y-axis Right"
;                set var.errorValue = var.deviationRightY
;                break
;            
;            if var.deviationU > var.tolerance
;                echo "ERROR: Deviation detected on U-axis: " ^ {var.deviationU} ^ "mm (tolerance: " ^ {var.tolerance} ^ "mm)"
;                set var.errorDetected = true
;                set var.errorAxis = "U-axis"
;                set var.errorValue = var.deviationU
;                break
;        else
;            ; Just show progress for non-accuracy-check iterations
;            echo "Iteration " ^ {iterations + 1} ^ " (Motion only - Temp: " ^ {var.chamber} ^ "°C)"
;    else
;        echo "ERROR: Failed to retrieve motion accuracy measurements"
;        set var.errorDetected = true
;        set var.errorAxis = "Data retrieval"
;        break

; ---- Test completion and cleanup ----
set var.XY_Mov_Time = state.time - var.XY_Mov_Start_Time

; Reset all temperatures and configurations
M570 H0 P30 T50 R10                                              ; configure heater fault detection
M570 H1 P30 T50 R10                                              ; configure heater fault detection

G10 P0 S0 R0
G10 P1 S0 R0
G10 P2 S0 R0
G10 P3 S0 R0
M140 S0 R0
M141 S0 R0

G90
G1 F18000 Y-999 X-150 U150
G1 Z300

; Final temperature readings
set var.t0 = sensors.analog[0].lastReading
set var.t1 = sensors.analog[1].lastReading  
set var.bed = sensors.analog[2].lastReading
set var.chamber = sensors.analog[3].lastReading

echo "Movement test completed in " ^ {var.XY_Mov_Time/60} ^ " minutes (" ^ {floor(var.XY_Mov_Time)} ^ " seconds)"
echo "Final Temperatures: Tool0=" ^ {var.t0} ^ "°C, Tool1=" ^ {var.t1} ^ "°C, Bed=" ^ {var.bed} ^ "°C, Chamber=" ^ {var.chamber} ^ "°C"
echo "Results saved to: Settings → Results → Motion_Accuracy_Test.csv"

; Success dialog with temperature data
var successMsg = "Movement test completed!<br><br>"
set var.successMsg = var.successMsg ^ "Duration: " ^ {floor(var.XY_Mov_Time/60)} ^ " min (" ^ {floor(var.XY_Mov_Time)} ^ "s)<br><br>"
set var.successMsg = var.successMsg ^ "Final Temps:<br>Tool0: " ^ {var.t0} ^ "°C, Tool1: " ^ {var.t1} ^ "°C<br>"
set var.successMsg = var.successMsg ^ "Bed: " ^ {var.bed} ^ "°C, Chamber: " ^ {var.chamber} ^ "°C<br><br>"
set var.successMsg = var.successMsg ^ "Temperature data saved to:<br>Settings→Results→Motion_Accuracy_Test.csv"

M291 S2 R"Motion Test Complete" P{var.successMsg}